
package BeamAndBall;
public class PI {
  private PIParameters pip;

  private double innerI; // Integrator state
  
  private double innerv; // Desired control signal
  private double innere; // Current control error
  
  public PI(String name) {
	  PIParameters p = new PIParameters();
	  p.innerBeta = 1.0;
	  p.innerH = 0.01;
	  p.innerIntegratorOn = false;
	  p.innerK = 1.0;
	  p.innerTi = 0.0;
	  p.innerTr = 10.0;
	  new PIGUI(this, p, name);
	  setParameters(p);
	  
	  this.innerI = 0.0;
	  this.innerv = 0.0;
	  this.innere = 0.0;
  }

  public synchronized double calculateOutput(double y, double yref) {
	  this.innere = yref - y;
	  this.innerv = pip.innerK * (pip.innerBeta * yref - y) + innerI; // I is 0.0 if integratorOn is false
	  return this.innerv;
  }

  public synchronized void updateState(double u) {
	  if (pip.innerIntegratorOn) {
		  innerI = innerI + (pip.innerK * pip.innerH / pip.innerTi) * innere + (pip.innerH / pip.innerTr) * (u - innerv);
	  } else {
		  innerI = 0.0;
	  }
  }

  public synchronized long getHMillis() {
  	return (long)(pip.innerH * 1000.0);
  }

  public synchronized void setParameters(PIParameters newParameters) {
  	pip = (PIParameters)newParameters.clone();
	if (!pip.innerIntegratorOn) {
		innerI = 0.0;
	}
  }
}
   
